In this chapter, an introduction to required background theory for this thesis is given.
The work presented in this thesis relies on \textbf{probabilistic robotics} (Thrun et al \cite{fox2005probabilistic}) and \textbf{computer vision} (Hartley and Zisserman \cite{Hartley2004}).
A clear vocabulary and corresponding mathematical nomenclature will be established that will be used consistently in the remainder of this thesis.
Probabilistic robots uses the notation of Thrun et al \cite{fox2005probabilistic} and computer vision uses the notation of Hartley and Zisserman \cite{Hartley2004}.

\section{Probabilistic robotics}
Sensors are limited in what they can percieve (e.g., the range and resolution is subject to physical limitations).
Sensors are also subject to noise, which deviates sensor measurements in unpredictable ways.
This noise limits the information that can be extraced from the sensor measurements.
Robot actuators (e.g., motors) are also subject to noise, which introduces uncertainty.
Another source of uncertainty is caused by the robot's software, which uses approximate models of the world.
Model errors are a source of uncertainty that has often been ignored in robots.
Robots are real-time systems, limiting the amount of computation that can be done.
This requires the use of algorithmic approximations, but increases the amount of uncertainty even more.

For some robotic applications (e.g., assembly lines with controlled environment), uncertainty is a marginal factor.
However, robots operating in uncontrolled environments (e.g., homes, other planets) will have to cope with significant uncertainty.
Since robots are increasingly deployed in the open world, the issue of uncertainty has become a major challenge for designing capable robots.
\textit{Probabilistic robots} is a relatively new approach to robotics that pays attentation to the uncertainty in perception and action.
Uncertainty is represented explicitly using the calculus of probability theory.
This means that probabilistic algorithms represent information by probability distributions over a space.
This allows to represent ambiguity and degree of belief in a mathematically sound way.
Now, robots can make choices (plan) relative to the uncertainty that remains, or chose to reduce the uncertainty (e.g., explore) if that is the best choice.

\subsection{Recursive state estimation}
\label{sec:background-recursive-state-estimation}
Environments are characterized by \textit{state}.
A state can be seen as a collection of all aspects of the robots and the environment that can impact the future.
The state also includes variables regarding the robot itself (e.g., pose, velocity, acceleration).
\begin{mydef}
Environments are characterized by \textbf{state}, which describes all aspects of a robot and the environment that can impact the future.
A state can be either \textbf{static state} (non-changing) or \textbf{dynamic state} when certain state variables tend to change over time.
A state is denoted $x$.
The state at time $t$ is denoted $x_t$.
\end{mydef}
Common state variables are:
\begin{itemize}
\item The robot \textit{pose}, which is its location and orientation (pitch, roll, yaw) relative to a global coodinate frame.
\item The \textit{robot velocity}, consisting of a velocity for each pose variable. A change in attitude is called \textit{angular velocity}.
\item The \textit{location and features of surrounding objects in the environment}. The objects can be either \textit{static} or \textit{dynamic}. For some problems, the objects are modelled as \textit{landmarks}, which are distinct, stationary features that provide reliable recognition.
\end{itemize}

A core idea of probabilistic robotics is the estimation of the state from sensor data.
Sensors observe only partial information about those quantities and their measurements are affected by noise. 
State estimation tries to recover state variables from the data.
Instead of computing a single state, a probability distribution is computed over possible world states.
This probability distribution over possible world states is called \textit{Belief} and is described on the next page.
\begin{mydef}
\textbf{State estimation} addresses the problem of estimating quantities from sensor data that are not directly observable, but can be inferred.
A probability distribution is computed over possible world states.
\end{mydef}

A robot can \textbf{interact} with its environment by influencing the state of its environment through its actuators (\textit{control actions}), or it can gather information about the state through its sensors (\textit{environment sensor measurements}).
\begin{mydef}
$z_{t_1:t_2} = z_{t_1}, z_{t_1 + 1}, z_{t_1 + 2}, \hdots, z_{t_2}$ denotes the set of all \textbf{measurements} acquired from time $t_1$ to time $t_2$, for $t_1 \leq t_2$.
\end{mydef}
\begin{mydef}
$u_{t_1:t_2} = u_{t_1}, u_{t_1 + 1}, u_{t_1 + 2}, \hdots, u_{t_2}$ denotes the sequence of all \textbf{control data} from time $t_1$ to time $t_2$, for $t_1 \leq t_2$.
\end{mydef}
The control data conveys information regarding the change of state.
For example, setting a robot's velocity at $1 \small{m/s}$, suggests that the robot's new position after $2$ seconds is approximately $2$ meters ahead of its position before the command was executed.
As a result of noise, $1.9$ or $2.1$ meters ahead are also likely new positions.
Generally, measurements provide information about the environment's state, increasing the robot's knowledge.
Motion tends to induce a loss of knowledge due to the inherent noise in robot actuation and the stochasticity of the environment.

The evolution of state and measurements is performed by \textbf{probabilistic laws}.
State $x_t$ is generated stochastically from the state $x_{t-1}$.
If state $x$ is complete (i.e., best predictor of the future), it is a sufficient summary of all that happened in previous time steps.
This assumption is known as the \textit{Markov Assumption} and is expressed by the following equality:
%and implies the current knowledge is sufficient to represent the past history of the robot.
\begin{equation}
\label{eq:background_state_trans_prob}
p(x_t | x_{0:t - 1}, z_{1:t - 1}, u_{1:t}) = p(x_t | x_{t-1}, u_t)
\end{equation}
where $p(x_t | x_{t-1}, u_t)$ is called \textit{state transition probability} and specifies how environmental state evolves over time as a function of robot control $u_t$.
%The state at time $t$ is stochastically dependent on the state at time $t - 1$ and the control $u$.

\begin{mydef}
The \textbf{Markov Assumption} states that past and future data are independently if one knows the current state $x$. This means the values in any state are only influenced by the values of the state that directly preceded it.
\end{mydef}

The process by which measurements are modelled is expressed by:
\begin{equation}
\label{eq:background_measurement_prob}
p(z_t | x_{0:t}, z_{1:t-1}, u_{1:t}) = p(z_t | x_t)
\end{equation}
where $p(z_t | x_t)$ is called \textit{measurement probability} and specified how measurements are generated from the environment state $x$.

Another core idea of probabilistic robotics is \textbf{belief}, which reflects the robot's knowledge about the environment's state.
Belief distributions are posterior probabilities over state variables conditioned on the available data.
The Markov Assumption implies the current belief is sufficient to represent the past history of the robot.
%Belief is represented through conditional probability distributions (posterior probabilities over state variables conditioned on the available data).
The belief over state variables is expressed with $bel(x_t)$:
%, which is an abbrevation for the posterior
\begin{equation}
bel(x_t) = p(x_t | z_{1:t}, u_{1:t})
\end{equation}
A \textit{prediction} of the state at time $t$ can be made before incorporating a measurement.
This prediction is used in the context of probabilistic filtering and is denoted as follows:
\begin{equation}
\overline{bel}(x_t) = p(x_t | z_{1:t-1}, u_{1:t})
\end{equation}
This equation predicts the state at time $t$ based on the previous state posterior, before incorporating the measurement at time $t$.
%A \textit{correction} or \textit{measurement update} is obtained by calculating $bel(x_t)$ from $\overline{bel}(x_t)$.

\begin{mydef}
\textbf{Belief} reflects the robot's knowledge about the environment's state, through conditional probability distributions.
Belief over state variable $x_t$ is denoted by $bel(x_t)$.
\end{mydef}

\subsubsection{Bayes filter}
The most general algorithm for calculcating beliefs is the \textbf{Bayes filter}.
The input of the algorithm is the (initial) belief $bel$ at time $t-1$, the most recent control $u_t$ and measurement $z_t$.
First, a \textit{prediction} for the new belief is computed based on control $u_t$ (the measurement is ignored):
\begin{equation}
\label{eq:background-theory-bayes-predict}
\overline{bel}(x_t) = \int p(x_t | u_t, x_{t-1}) bel(x_{t-1}) dx_{t-1}
\end{equation}
The second step of the Bayes filter is the \textit{measurement update}:
\begin{equation}
\label{eq:background-theory-bayes-measurement}
bel(x_t) = \eta p(z_t | x_t) \overline{bel}(x_t)
\end{equation}
Where $\eta$ is a normalization constant to ensure a probability.
Now, $bel(x_t)$ is the robot's belief about the state after the measurement and control data are used to improve the robot's knowledge.
Both steps of the algorithm are performed recursively for all measurements and controls.

The most popular family of recursive state estimators are the \textbf{Gaussian filters}.
Gaussian filters represent belief by multivariate normal distributions (approximated by a Gaussian function).
The density (probability) over the state space $x$ is characterized by two parameters: the mean $\mu$ and the covariance $\Sigma$.
The dimension of the covariance matrix is is the dimensionality of the state $x$ squared.
Representing the posterior by a Gaussian has some important consequences.
First, Gaussians are unimodel and have a single maximum.
However, this is characterisic for many tracking problems, where the posterior is focussed around the true state with a certain margin of uncertainty.

In order to implement the filter described above, two additional components are required.
In Section \ref{sec:motion_model}, the \textit{motion model} is described.
In Section \ref{sec:measurement_model}, the \textit{measurement model} is described.

		\subsection{Motion model}
		\label{sec:motion_model}
Probabilistic motion models describe the relationship between the previous and current pose and the issued controls (commands) by a probability distribution. This is called the \textit{state transition probability}.
The state transition probability plays an essential role in the prediction step of the Bayes filter.
\begin{mydef}
A probabilistic \textbf{motion model} defines the state transition between two consecutive time steps $t-1$ and $t$ after a control action $u_{t}$ has been carried out. It is expressed by the posterior distribution $p (x_t | x_{t-1}, u_{t})$.
\end{mydef}
%In order to derive $p (x_t | x_{t-1}, u_{t})$, it is necessary to derive the kinematics from its mechanics and its %possible sources of noise and deviation.

Two complementary probabilistic motion models are the \textit{Velocity Motion Model} and the \textit{Odometry Motion Model}.
The Velocity Motion Model assumes that a robot is controlled through two velocities: a rotational velocity $\omega_t$ and translational velocity ($v_t$).
\begin{equation}
u_t = 
\left( \begin{array}{c}
v_t \\
\omega_t \end{array} \right)
\end{equation}
Algorithms for computing the probability $p (x_t | x_{t-1}, u_{t})$ can be found in \cite{fox2005probabilistic}.
The Odometry Motion Model assumes that one has access to odometry information (e.g., wheel encoder information).
In practice, odometry models tend to be more accurate, because most robots do not execute velocity command with the level of accuracy that is obtained by measuring odometry.
Technically, odometry reading are not controls because they are received after executing a command.
However, using odometry readings as controls results in a simpler formulation of the estimation problem.

Both motion models are not directly applicable to MAVs.
For example, quadrotor helicopters are controlled through pitch, roll and yaw (as explained in Section \ref{sec:platform-quadrotor-flight-control}).
A conversion between angles to velocities is required to use the Velocity Motion Model for a quadrotor helicopter.
For ground robots, the odemetry reading can be obtained by integrating wheel encoder information.
Because a quadrotor helicopter has no contact with the ground, odometry readings are not directly available.
Instead, different sources of information are used to obtain odometry information.
For example, the AR.Drone uses a camera that points downwards to recover odometry information (Section \ref{sec:platform-visual-odometry}).


		\subsection{Measurement model}
		\label{sec:measurement_model}
Probabilistic measurement models describe the relationship between the world state $x_t$ and how a sensor reading (observation) $z_t$ is formed.
Based on the estimated world state $x_t$, a belief over possible measurements is generated.
Noise in the sensor measurements is modelled explicitly, inherent to the the uncertainty of the robot's sensors.
To express the process of generating measurements, a specification of the environment is required.
A map $m$ of the environment is a list of objects and their locations:
\begin{equation}
m = \{m_1, m_2, \hdots, m_N\}
\end{equation}
Where $N$ is the total number of objects in the environment.
Maps are often \textit{Features-based Maps} or \textit{Location-based maps}.
Location-based maps are volumetric and offer a label for any location.
Features-based Maps only describe the shape of the environment at specific locations, which are commonly objects.
Features-based maps are more popular because the representation makes it easy to adjust (refine) the position of objects.
\begin{mydef}
A probabilistic \textbf{sensor model} describes the relation between a world state $x_t$ and a sensor reading $z_t$ given an environmental model $m$ in form of a posterior distribution $p (z_t | x_t, m)$.
\end{mydef}


		\subsection{Localization}
\textit{Robot localization} is the problem of estimating the robot's pose (state) relative to a map of the environment.
Localization is an important building block for successful navigation of a robot, together with \textit{perception}, \textit{cognition} and \textit{motion control}.

When a robot is moving in a known enviroment and starting at a known location, it can keep track of its location by integrating local position estimates (e.g., odometry).
Due to uncertainty of these local estimates, the uncertainty of the robot's absolute location increases over time.
In order to reduce the growing uncertainty, the robot has to retrieve its absolute location by localizing itself in relation to a map.
To do so, the robot uses its sensors to make observations of its environment and relate these observations to a map of the environment.

A probabilistic approach to localization uses belief to represent the estimated global location.
Again, updating the robot's position involves two steps.
The first step is called the \textit{prediction update}.
The robot uses its state at time $t - 1$ and control data $u_t$ to predicts its state at time $t$.
This prediction step increases the uncertainty about the robot's state.
%The uncertainty about the robot's state increases due to integration of the odometric error over time.
The second step is called the \textit{perception update}.  
In this step, the robot uses the information from its sensors to correct the position estimated during the prediction phase, reducing the uncertainty about the robot's state.

\subsection{Simultaneous Localization and Mapping}

A more difficult subclass of localization occurs when a map of the environment is not available.
In this case, the robot's sensor information is used to both recover the robot's path and build a map of the environment.
In the robotics community, this problem is called \textbf{Simultaneous Localization and Mapping (SLAM)}.
A solution to this problem would make a robot truly autonomous.
SLAM is a difficult problem because both the estimated path and constructed map are affected by noise.
Both of them become increasingly inaccurate during travel.
However, when a place that has been mapped is revisited, the uncertainty can be reduced.
This processes is called \textit{loop-closing}.
Additionally, the map can be optimized after a loop-closure event is detected.


\subsubsection{Solution techniques}
\label{sec:background-solution-techniques}
In practice, it is not possible to compute a robots belief (posterior probabilities) analytically.
%the posterior (belief) analytically.
Therefore, a number of approximation techniques exist. 
This section describes the most popular solutions techniques for SLAM problems.

Solution techniques can be divided in two major branches of belief representation: \textit{Single-hypothesis belief} and \textit{Multi-hypothesis belief}.
\textbf{Single-hypothesis trackers} represent belief by a single world state.
This estimate is associated by a measure of certainty (or variance), allowing the tracker to broaden or narrow the estimated region in the state space.
The main advantage of the single-hypothesis representation is the absence of ambiguity, simplifying decision-making at the robot's cognitive level (e.g., pathplanning).
Due to the absence of ambiguity, the trackers fails to model ambiguities adequately.
%An example is a soccer field, which is a symmetrical environment.

\textbf{Multi-hypothesis trackers} represent belief not just as a single world state, but as a possibly infinite set of states.
The main advantage of the multi-hypothesis representation is that the robot can explicitly maintain uncertainty regarding its state.
This allows a robot to believe in multiple poses simultaneously, allowing the robot to track and reject different hypotheses independently.
One of the main disadvantages of a multi-hypothesis representation involves decision making.
If the robot represents its position as a set of points, it becomes less obvious to compute a next move.

Futhermore, solution techniques can be divided in two branches of time constraints: \textit{online computing} and \textit{offline computing}.
\textbf{Online} techniques solve a problem in real-time and must guarantee response within strict time constraints.
One advantage of online techniques is the ability to use the response as input for decision making (navigation), which is essential for autonomous robots.
Due to the strict time constraints, a limited amount of computations can be performed, which limits the complexity of solution techniques.

\textbf{Offline} techniques have less strict time contraints, which allows increased complexity.
For example, additional refinements or optimizations can be performed that would be too slow for online techniques.
One of the main disadvantages of offline techniques involves decision making.
Because a response is not available in real-time, the robot has to wait or make a decision without incorporating the response.


\paragraph{Kalman Filter (KF)}
The most popular technique for implementing a Bayes filter is probably the Kalman Filter (1960) \cite{kalman1960new}, often used for improving vehicle navigation.
The filter has a Single-hypothesis belief representation and can be performed in real-time.
The Kalman Filter is based on the assumption that the system is linear and both the motion model and measurement model are affected by white Gaussian noise.
Belief at time $t$ is represented by a multivariate Gaussian distribution defined by its mean $\mu_t$ and covariance $\Sigma_t$.
This means the $x_t$ from the Bayes filter \eqref{eq:background-theory-bayes-predict} and \eqref{eq:background-theory-bayes-measurement}  has been replaced by $\mu_t$ and $\Sigma_t$.
%This representation limits the filter to continuous states.

The Kalman Filter assumes the system is linear: the state transition $A$ (Equation \ref{eq:background_state_trans_prob}), the motion model $B$ and the sensor model $C$ (Equation \ref{eq:background_measurement_prob}) are linear functions solely depending on the state $x$ or control command $u$, plus a Gaussian noise model $Q$:
\begin{equation}
\label{eq:KF}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\mu}_t & = & A_t \mu_{t-1} + B_t u_t  & \text{a-priori mean estimate} \\
\overline{\Sigma}_t & = & A_t \Sigma_{t-1} A_t^T + R_t  & \text{a-priori covariance estimate} \\
\text{\textbf{Update}} \\
K_t & = & \overline{\Sigma}_t C_t^T (C_t \overline{\Sigma}_t C_t^T + Q_t)^{-1} & \text{Kalman gain} \\
\mu_t & = & \overline{\mu}_t + K_t(z_t - C_t \overline{\mu}_t) & \text{updated (a posteriori) state estimate} \\
\Sigma_t & = & (I - K_t C_t) \overline{\Sigma}_t & \text{updated (a posteriori) estimate covariance}
\end{array}
\end{equation}

The Kalman Filter presesents the belief $bel(x_t)$ at time $t$ by the mean $\mu_t$ and covariance $\Sigma_t$.
When a measurement is received, a new belief $bel(x_t)$ is predicted based on the previous belief bel($x_{t-1}$), the control data $u_t$ and both the state transition model and motion model.
This predicted belief describes the most likely state $\overline{\mu}_t$ at time $t$ and the covariance $\overline{\Sigma}_t$.
The measurement is not yet included, because its a predicted belief.
The update step transforms the predicted belief ($\overline{\mu}_t$, $\overline{\Sigma}_t$) into the desired belief ($\mu_t$, $\Sigma_t$), by incorporating the measurement $z_t$.
The Kalman gain $K_t$ specifies the degree to which the measurement is incorporated into the new state estimate.
The key concept used for Kalman Filtering is the \textit{innovation}, which is the difference between the actual measurement $z_t$ and the expected measurement $C_t \overline{\mu}_t$, which is derived from the predicted state.
A measurement $z_t$ which is far off the predicted measurement, is less reliable, thus less incorporated in the new state estimate.

The standard Kalman Filter requires a linear system (.i.e, a linear combination of Gaussians results in another Gaussian), which is insufficient to describe many real-life problems.
Therefore, variations of the original algorithm have been proposed that can cope with different levels of non-linearity.
These variations approximate the motion and sensor models in a way to make them linear again.

\paragraph{Extended Kalman Filter (EKF)}
The Extended Kalman Filter is a variant on the Kalman Filter that can be used for non-linear systems.
It tries to approximate \textit{non-linear} motion and sensor models to make them linear.
The state transition probability and the measurement probabilities are governed by the non-linear functions $g$ and $h$:
\begin{equation}
\begin{array}{rcl}
x_t & = & g(u_t, x_{t-1}) \\
z_t & = & h(x_t) + \delta_t
\end{array}
\end{equation}
These functions replace $A$, $B$ and $C$ of the regular Kalman Filter (Equation \ref{eq:KF}).

They key idea of the EKF approximation is called \textit{linearization}.
A first-order linear Taylor approximation is calculated at the mean of the current belief (Gaussian), resulting in a linear function.
Projecting the Gaussian belief through the linear approximation results in a Gaussian density.
\begin{equation}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\mu}_t & = & \boldsymbol{g(u_t, \mu_{t-1})} & \text{a-priori mean estimate} \\
\overline{\Sigma}_t & = & \boldsymbol{G_t} \Sigma_{t-1} \boldsymbol{G_t^T} + R_t  & \text{a-priori covariance estimate} \\
\text{\textbf{Update}} \\
K_t & = & \overline{\Sigma}_t \boldsymbol{H_t^T} (\boldsymbol{H_t} \overline{\Sigma}_t \boldsymbol{H_t^T} + Q_t)^{-1} & \text{Kalman gain} \\
\mu_t & = & \overline{\mu}_t + K_t(z_t - \boldsymbol{h(\overline{\mu}_t)}) & \text{updated (a posteriori) state estimate} \\
\Sigma_t & = & (I - K_t \boldsymbol{H_t}) \overline{\Sigma}_t & \text{updated (a posteriori) estimate covariance}
\end{array}
\end{equation}



In this thesis a Single-hypothesis belief representation was chosen for its simplicity.
Furthermore, all work presented in this thesis has a strong emphasis on online techniques, allowing the work to be used for navigation tasks like the IMAV Pylon challenge (Section \ref{sec:introduction-imav}).

\begin{comment}
\subsubsection{Single-hypothesis trackers}
Single-hypothesis trackers represent belief by a single world state.
This estimate is associated by a measure of certainty (or variance), allowing the tracker to broaden or narrow the estimated region in the state space.
The main advantage of the single-hypothesis representation is the absence of ambiguity, simplifying decision-making at the robot's cognitive level (e.g., pathplanning).
Updating the robot's belief is also simplified, because a single state must be updated to a new single state.
However, the main disadvantage of a single-hypothesis representation is that a update process always has to generate a \textit{single} hypothesis, which is challenging and sometimes impossible.

\paragraph{Kalman Filter (KF)}
The most popular technique for implementing a Bayes filter is probably the Kalman Filter (1960) \cite{kalman1960new}, often used for improving vehicle navigation.
The Kalman Filter is based on the assumption that the system is linear and both the motion model and measurement model are affected by white Gaussian noise.
Belief at time $t$ is represented by a multivariate Gaussian distribution defined by its mean $\mu_t$ and covariance $\Sigma_t$.
This representation limits the filter to continuous states.

The Kalman Filter assumes the system is linear: the state transition $A$ (Equation \ref{eq:background_state_trans_prob}), the motion model $B$ and the sensor model $C$ (Equation \ref{eq:background_measurement_prob}) are linear functions solely depending on the state $x$ or control command $u$, plus a Gaussian noise model $Q$:
\begin{equation}
\label{eq:KF}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\mu}_t & = & A_t \mu_{t-1} + B_t u_t  & \text{a-priori mean estimate} \\
\overline{\Sigma}_t & = & A_t \Sigma_{t-1} A_t^T + R_t  & \text{a-priori covariance estimate} \\
\text{\textbf{Update}} \\
K_t & = & \overline{\Sigma}_t C_t^T (C_t \overline{\Sigma}_t C_t^T + Q_t)^{-1} & \text{Kalman gain} \\
\mu_t & = & \overline{\mu}_t + K_t(z_t - C_t \overline{\mu}_t) & \text{updated (a posteriori) state estimate} \\
\Sigma_t & = & (I - K_t C_t) \overline{\Sigma}_t & \text{updated (a posteriori) estimate covariance}
\end{array}
\end{equation}

The Kalman Filter presesents the belief $bel(x_t)$ at time $t$ by the mean $\mu_t$ and covariance $\Sigma_t$.
When a measurement is received, a new belief $bel(x_t)$ is predicted based on the previous belief bel($x_{t-1}$), the control data $u_t$ and both the state transition model and motion model.
This predicted belief describes the most likely state $\overline{\mu}_t$ at time $t$ and the covariance $\overline{\Sigma}_t$.
The measurement is not yet included, because its a predicted belief.
The update step transforms the predicted belief ($\overline{\mu}_t$, $\overline{\Sigma}_t$) into the desired belief ($\mu_t$, $\Sigma_t$), by incorporating the measurement $z_t$.
The Kalman gain $K_t$ specifies the degree to which the measurement is incorporated into the new state estimate.
The key concept used for Kalman Filtering is the \textit{innovation}, which is the difference between the actual measurement $z_t$ and the expected measurement $C_t \overline{\mu}_t$, which is derived from the predicted state.
A measurement $z_t$ which is far off the predicted measurement, is less reliable, thus less incorporated in the new state estimate.

The standard Kalman Filter requires a linear system (.i.e, a linear combination of Gaussians results in another Gaussian), which is insufficient to describe many real-life problems.
Therefore, variations of the original algorithm have been proposed that can cope with different levels of non-linearity.
These variations approximate the motion and sensor models in a way to make them linear again.

\paragraph{Extended Kalman Filter (EKF)}
The Extended Kalman Filter is a variant on the Kalman Filter that can be used for non-linear systems.
It tries to approximate \textit{non-linear} motion and sensor models to make them linear.
The state transition probability and the measurement probabilities are governed by the non-linear functions $g$ and $h$:
\begin{equation}
\begin{array}{rcl}
x_t & = & g(u_t, x_{t-1}) \\
z_t & = & h(x_t) + \delta_t
\end{array}
\end{equation}
These functions replace $A$, $B$ and $C$ of the regular Kalman Filter (Equation \ref{eq:KF}).

They key idea of the EKF approximation is called \textit{linearization}.
A first-order linear Taylor approximation is calculated at the mean of the current belief (Gaussian), resulting in a linear function.
Projecting the Gaussian belief through the linear approximation results in a Gaussian density.
\begin{equation}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\mu}_t & = & \boldsymbol{g(u_t, \mu_{t-1})} & \text{a-priori mean estimate} \\
\overline{\Sigma}_t & = & \boldsymbol{G_t} \Sigma_{t-1} \boldsymbol{G_t^T} + R_t  & \text{a-priori covariance estimate} \\
\text{\textbf{Update}} \\
K_t & = & \overline{\Sigma}_t \boldsymbol{H_t^T} (\boldsymbol{H_t} \overline{\Sigma}_t \boldsymbol{H_t^T} + Q_t)^{-1} & \text{Kalman gain} \\
\mu_t & = & \overline{\mu}_t + K_t(z_t - \boldsymbol{h(\overline{\mu}_t)}) & \text{updated (a posteriori) state estimate} \\
\Sigma_t & = & (I - K_t \boldsymbol{H_t}) \overline{\Sigma}_t & \text{updated (a posteriori) estimate covariance}
\end{array}
\end{equation}

\paragraph{Unscented Kalman Filter (UKF)}
A drawback of the Extended Kalman Filter is that the true mean and covariance of the posterior distribution is not preserved.
Another variant on the Kalman Filter has been found to yield superior results.
The \textit{Unscented Kalman Filter (UKF)} performs a stochastic linearization using a weighted statistical linear regression process.
The \textit{unscented transform} sampling technique is used to pick a minimal set of sigma points around the mean.
These sigma points are propagated through the non-linear function $g$, from which the mean and covariance of the estimate are then recovered.
Each sigma point has two weights associated with it. One weight is for recovering the mean, the other weight is used for recovering the covariance.

In practice, the EKF is sightly faster than the UKF.
The UKF produces better results than the EKF, depending on the non-linearities and spread of the prior state uncertainty.
However, in many practical applications, the different is very small.


\paragraph{Information Filter (IF)}
The dual of the Kalman Filter is the \textit{Information Filter}.
The key different between both filters lies in the way the Gaussian belief is represented.
Kalman Filters respresent Gaussians by their moments (mean and covariance).
Information Filters present Gaussians by their canonical parameterization (information matrix and information vector).
\begin{equation}
\label{eq:KF}
\begin{array}{rclr}
\text{\textbf{Predict}} \\
\overline{\Omega}_t & = & (A_t \Omega_{t-1}^{-1} A_t^T + R_t)^{-1}  & \text{a-priori information matrix} \\
\overline{\xi}_t & = & \overline{\Omega}_t ( A_t \Omega_{t-1}^{-1} \xi_{t-1} + B_t u_t )  & \text{a-priori information vector} \\
\text{\textbf{Update}} \\
\Omega_t & = & \C_t^T Q_t^{-1} \C_t + \overline{\Omega}_t & \text{updated information matrix} \\
\xi_t & = & C_t^T Q_t^{-1} z_t + \overline{\xi}_t & \text{updated information vector}
\end{array}
\end{equation}

The difference in parameterization leads to different computational complexities.
The prediction step of the Kalman Filter is additive, which makes is computationally inexpensive.
However, the measurement update in Kalman Filter is computationally complex, due to the requirement of matrix inversion.
For the Information Filter, the complexities are exactly opposite.
The prediction step of the Information Filter involves the inversion of two matrices, which can be complex.
On the other hand, the measurement update of the Information Filter is additive, which makes it inexpensive.
The main advantage of the Information Filter is that $N$ measurements can be filtered at each timestep simply by summing their information matrices and vectors.
This makes Information Filters particularly interesting for scenarios where multiple robots have to merge their local information with the information collected by other robots.


\begin{comment}
\subsubsection{Multi-hypothesis trackers}


\paragraph{Particle Filter}
The key idea of the \textit{Particle Filter}  is to represent the posterior $bel(x_t)$ by a set of random state particles (samples) drawn from this posterior.
This representation is approximate, but can represent a much broader space of discributions than the single-hypothesis trackers.
Another advantage is that the transformations are not limited to linear functions anymore.

The particles (samples) are denoted
\begin{equation}
\chi = x_t^{[1]}, x_t^{[2]}, \hdots, x_t^{[M]}
\end{equation}
Each particle $x_t^{[m]}$ is a state hypothesis at time $t$.
Ideally, the likelihood for a state hypothesis $x_t$ to be in $\chi$, should be proportional to $bel(x_t)$.
The denser a subregion of the the space is populated with particles, the more likely it is that the true state falls into this region.
In the prediction step, the motion model is applied to each particle $x_{t-1}^{[m]}$ to generate a prediction $x_t^{[m]}$.
When observation $z_t$ is made, the \textit{importance factor} $w_t^{[m]}$ is computed.
\begin{equation}
w_t^{[m]} = p ( z_t | x_t^{[m]}, z_{0 \to t-1})
\end{equation}
The final step of the particle filter is {resampling}, in which the current set of particles re-sampled based on the computed likelihoods.
The algorithm draws with replacement $M$ particles from the temporary set $\overline{\chi}_t$.
The probability of each particle is given by its importance weights.
This changes the distribution of the particle from $\overline{bel}(x_t)$ to $bel(x_t)$.

Particle filters overcome the limitations of using a Gaussian to represent a probability distribution.
With sufficient samples, they approach the Bayesian optimal estimate, so they can be made more accurate than either the EKF or UKF.
The increased representational power of particle filters comes at the cost of higher computational complexity.
And, when the number of particles is unsufficient, the particle filter might suffer from sample impoverishment.
\end{comment}


\section{Computer vision}
\label{sec:background-computer-vision}
Laser range scanners are often used for robot localization.
However, this sensor has a number of problems (e.g., limited range, deal with dynamic environments).
Vision has long been advertised as a solution.
A camera can make a robot perceive the world in a way similar to humans.

\textbf{Computer vision} is a field that includes methods for acquiring, processing, analysing, and understanding images.
Since a camera is the AR.Drone's only sensor that is capable of retrieving detailed information of the environment, computer vision techniques are essential when performing robot localization with the AR.Drone.
In addition, vision techniques can be used to calibrate other sensors of a robot.

\subsection{Projective geometry}
\label{sec:background-projective-geometry}
Projective geometry describes the relation between a scene and how a picture of it is formed.
A projective transformation is used to map objects onto a picture.
This transformation preserves straightness.
Multiple geometrical systems can be used to describe geometry.

\textbf{Euclidean geometry} is a popular mathematical system.
A point in Euclidean 3-space is represented by an unordered pair of real numbers, $(x, y, z)$.
However, this coordinate system is unable to represent points vanishing points.
The \textit{Homogeneous coordinates} coordinate system is able to represent points at infinity.
Homogeneous coordinates have an additional coordinate.
Now, points are represented by \textit{equivalance classes}, where points are equivalant when they differ by a common multiple.
For example, $(x, y, z, 1)$ and $(2x, 2y, 2z, 2)$ represent the same point.
Points at infinity are respresented using $(x, y, z, 0)$, because $(x/0, y/0, z/0)$ is infinite.
In Euclidean geometry, one point is picked out as the \textit{origin}.
A coordinate can be transformed to another coordinate by translating and rotating to a diffent position.
This operation is know as a \textbf{Euclidian transformation}.
A more general type of transformation is an \textbf{affine transformation}, which is linear transformation (linear stretching), followed by a Euclidian transformation.
The resulting transformation performs moving, rotating and finally stretching linearly possibly by different ratios in different directions.

In computer vision problems, projective space is used as a convenient way of representing the real 3D world.
A \textbf{projective transformation} of projective space $\mathds{P}^n$ is represented by a linear transformation of homogeneous coordinates:
\begin{equation}
X' = H_{(n+1)} \times _{(n+1)}X
\end{equation}
where $X$ is a coordinate vector and $H$ is a non-singular matrix.

In order to analyze and manipulate images, understanding about the image formation process is required.
Light from a scene reaches the camera and passes through a lens before it reached the sensor.
The relationship between the distance to an object $z$ and the distance behind the lens $e$ at which a focuses image is formed, can be expresses as:
\begin{equation}
\label{eq:optics}
\frac{1}{f} = \frac{1}{z} + \frac{1}{e}
\end{equation}
where $f$ is the focal length.

The \textbf{pinhole camera} has been the first example of camera in the history \cite{gernsheim1969history}.
%\footnote{\url{http://inventors.about.com/od/pstartinventions/a/stilphotography.htm}}
It is a lightproof box with a very small aperture instead of a lens.
Light from the scene passes through the aperture and projects an \textit{inverted image} on the opposit side of the box.
This principle has been adopted as a standard model for perspective cameras.
In this model, \textit{optical center} $C$ (apeture) corresponds to the center of the lens.
The intersection $O$ between the the optical axis and the image plane is called \textit{principal point}.
The pinhole model is commonly described with the image plane between $C$ and the scene in order to preserve the same orientation as the object.

The operation performed by the camera is called the \textbf{perspective transformation}.
It transforms the 3D coordinate of scene point $P$ to coordinate $p$ on the image plane.
A simplified version of the perspective transformation can be expressed as:
\begin{equation}
\frac{f}{P_z} = \frac{p_x}{P_x} = \frac{p_y}{P_y}
\end{equation}
from which $p_x$ and $p_y$ can be recovered:
\begin{equation}
p_x = \frac{f}{P_z} \cdot P_x
\end{equation}
\begin{equation}
p_y = \frac{f}{P_z} \cdot P_y
\end{equation}
When using homogeneous coordinates, a linear transformation is obtained:
\begin{equation}
\left[ \begin{array}{c}
\lambda p_x \\
\delta p_y \\
\delta
\end{array} \right]
=
\left[ \begin{array}{c}
f P_x \\
f P_y \\
P_z
\end{array} \right]
=
\left[ \begin{array}{c c c c}
f & 0 & 0 & 0  \\
0 & f & 0 & 0 \\
0 & 0 & 1 & 0
\end{array} \right]
\left[ \begin{array}{c}
P_x \\
P_y \\
P_z \\
1
\end{array} \right]
\end{equation}
From this equation can be seen that it is not possible to estimate the distance to a point (i.e., all points on a line project to a single point on the image plane).

A more realistic camera model is called the \textbf{general camera model}.
It takes into account the rigid body transformation between the camera and the scene, and \textit{pixelization} (i.e.e, shapes and position of the CCD sensor).
Pixelization is addressed by replacing the focal length matrix with a \textit{camera intrinsic parameter matrix} $A$.
The rigid body transformation is adressed by adding a combined rotation and translation matrix $[R|t]$, which are called the \textit{camera extrinsic parameters}.
\begin{equation}
\lambda p = A [ R | t] P
\end{equation}
or
\begin{equation}
\left[ \begin{array}{c}
\lambda p_x \\
\delta p_y \\
\delta
\end{array} \right]
=
\left[ \begin{array}{c c c}
\alpha_u & 0 & u_0 \\
0 & \alpha_v & v_0 \\
0 & 0 & 1
\end{array} \right]
\left[ \begin{array}{c c c c}
r_{11} & r_{12} & r_{13} & t_1 \\
r_{21} & r_{22} & r_{23} & t_2 \\
r_{31} & r_{32} & r_{33} & t_3
\end{array} \right]
\left[ \begin{array}{c}
P_x \\
P_y \\
P_z \\
1
\end{array} \right]
\end{equation}

\textbf{Camera calibration} is the process of measuring the intrinsic and extrinsic parameters of the camera.
As explained in the previous paragraph, these parameters are required to calculcate the mapping between 3D scene points to 2D points on the image plane.
When scene points $P$ and image points $p$ are known, it is possible to compute $A$, $R$, $t$ by solving the perspective projection equation.
Newer camera calibration techniques use a planar grid (e.g., chessboard-like pattern) instead of 3D points, to ease the extraction of corners.
The method requires serveral pictures of the pattern shown at different positions and orientations.
Because the 2D positions of the corners are known and matched between all images, the instrinsic and extrinsic parameters are determined simultaneously by solving a least-square minimization.

\subsection{Feature extraction and matching}
In order to perform robot localization using vision sensors, the robot must be able to relate recent camera frames to previously received camera frames (map).
However, all measurents have an error, which complicates the matching.
A strategy to deal with uncertainty is by generating a higher-level description of the sensor data instead of using the raw sensor data.
This process is called \textit{feature extraction}.

\begin{mydef}
\textbf{Features} are recognizable structures of elements in the environment.
They are extracted from measurements and described in a mathematical way.
\textit{Low-level features} are geometric primitives (e.g., lines, points, corners) and \textit{high-level features} are objexts (e.g., doors, tables).
\end{mydef}

Features play an essential role in mobile robots.
They enable more compact and robust descriptions of the environment.
Choosing the approriate features is a critical task when designing a mobile robot.

\subsubsection{Feature extraction}
\label{sec:background-feature-extraction}
A local feature is small image patch that differs that is different than its immediate neighborhood.
Difference can be in terms on intensity, color and texture.
Commonly, feature are edges, corners or junctions.
The most important aspect of a feature detector is \textit{repeatability}, which means the detector is able to detect the same features, when multiple images (with different viewing and illumuniation conditions) of the same scene are used.
Another important aspect of a feature detector is \textit{distinctiveness}, which means the information carried by a patch is distinctive as possible.
This is important for robust feature matching.
Good feature are \textit{invariant}, meaning that changes in camera viewpoint, illumination, rotation and scale do not affect a feature and its high-level description.

\textbf{Scale-invariant feature transform (SIFT)} \cite{lowe1999object} is the most popular feature extractor.
It is invariant to scale, rotation, illumination and viewpoint.

The algorithm can be outlined as follows.
First, an internal representation is computed to ensure scale invariance.
Secondly, an approximation of the Laplacian of Gaussian is used to find interesting keypoints.
Third, keypoints are found at maxima and minima in the Difference of Gaussian from the previous step.
Fourth, bad keypoints (e.g., edges and low contrast regions are eliminated.
Fifth, an orientation is calculated for each keypoint.
Any further calculations are done relative to this orientation, making it rotation invariant.
FInally, a high-level representation is calculated to uniquely identify features.
This \textit{feature descriptors} is a vector of 128 elements, containing orientation histograms.

For some applications or mobile platforms, the SIFT extractor is too slow.
Therefore, some faster variants are proposed, like \textbf{Speeded Up Robust Feature (SURF)} \cite{Bay2008cviu}.
%SURF is partly inspired by the SIFT descriptor.
The standard version of SURF is several times faster than SIFT and claimed by its authors to be more robust against different image transformations than SIFT.
The important speed gain is achieved by using integral images, which drastically reduce the number of operations for simple box convolutions, independent of the chosen scale.

The algorithm can be outlined as follows.
Initially, an integral image and  Hessian matrix are computed.
Blob-like structures are detected at locations where the determinant is maximum.
The images are repeatedly smoothed with a Gaussian and then sub-sampled in order to create pyramid of different resolutions.
In order to localise interest points in the image and over scales, a non-maximum suppression in a $3 \times 3 \times 3$ neigh bourhood is applied. 
The maxima of the determinant of the Hessian matrix are then interpolated in scale and image space.
A descriptor is construced that describess the distribution of the intensity content within the interest point neighbourhood.
First-order Haar wavelet responses in $x$ and $y$ direction are used rather than the gradient, exploit integral images for speed. The size of the descriptor is reduced to $64$ dimensions.

\textbf{GIST}

\subsubsection{Feature matching}
\label{sec:theory_feature_matching}
To perform robot localization, extracted feature descriptors need to matched against the feature descriptors that are part of the map.
Ideally, each feature extracted from the current frame is correlated against the corresponding feature inside the map.

A \textit{matching function} is used to compute the correspondance between two feature descriptors.
The matching function usually depends on the type of feature descriptor.
In general, a feature descriptor is a high-dimensional vector, and matching features can be found by computing the distance using the $L_2$ norm, which is defined as follows:
\begin{equation}
L2(a,b) =\sqrt { \sum_{i=1}^{N} \left| a_i - b_i \right| ^2 }
\end{equation}
where $N$ is the dimensionality of the descriptor vector.

The matching function is used to find matches between two sets of descriptors.
A popular matcher is the \textit{Brute-force descriptor matcher}. For each descriptor in the first set, this matcher finds the closest descriptor in the second set by trying each one.
However, the brute-force matcher becomes very slow for large descriptors sets, due to the complexity of $O(n^2)$.
For large descriptor sets, the \textit{Fast Library for Approximate Nearest Neighbors (FLANN)} \cite{muja2009flann} matcher performs more efficient matching.
It uses an Approximate Nearest Neighbors algorithm to find similar descriptors efficiently.
The authors state that the approximation has proven to be a good-enough in most practical applications and in most cases, orders of magnitude faster that the algorithms performing the exact searches.

\subsubsection{Popular application: image stitching}
A popular computer vision application that heavily relies on features is \textit{image stiching}, which is the process of combining multiple photos with overlapping fields of view.
The result is a segmented panorama or high-resolution image. 

The first step involves extracting features for all images.
Since most features are invariant under rotation and scale changes, images with varying orientation and zoom can be used for stitching.
%These features are matched to search for image alignments that minimize the sum of absolute differences between overlapping pixels.
Images that have a large number of matches between them are identified.
These image are likely to have some degree of overlap.
A homography $\boldsymbol{H}$ (projective transformation) is computed between matching image pairs.
This homography described how the second image needs to be transformed in order to correctly overlap the first image.
Different types of homographies can be used to describe the transformation between image pairs, depending on the level of displacement between images.

$\boldsymbol{H}$ cannot be computed from all feature matches directly.
False matches would reduce the accuracy of the estimated homography.
Instead, a robust estimation procedure is applied.

\textbf{RANSAC (Random Sample Consensus)} \cite{fischler1981random} is a simple robust estimation procedure that uses a minimal set of randomly sampled correspondences to estimate image transformation parameters, and finds a solution that has the best consensus with the data.
Commonly, 4 random feature correspondences are used to compute the homography $\boldsymbol{H}$ of an image pair.
This step is repeated $N$ trials and the solution that has the maximum number of inliers (whose projections are consistent with $\boldsymbol{H}$ within a tolerance $\epsilon$ pixels) is selected.

When the homographies of all image pairs are estimated, a global optimization method can be applied to reduce drift or other sources of error.
Finally, the images are transformed according to the corresponding homographies, resulting in a seamless panorama or high-resolution image. 


\subsection{Visual Odometry}
\label{sec:background-visual-odometry}
An interesting combination of computer vision and mobile robotics is \textit{Visual Odometry (VO)}, which is the process of estimating the egomotion of a mobile robot using one or more onboard cameras.
Below, only the case of a single camera and 2-D feature coordinates is described.
There are two main approaches two compute the relative motion from video images: \textit{appearance-based} methods and \textit{feature-based} methods.
Appearance-based methods use intensity information, while feature-based methods use higher-level features.
Feature-based methods are more popular, because they are faster and more accurate than appearance-based methods.

The set of images taken at time $t$ is denoted by $I_{0:n} = \{I_0, \hdots, I_n\}$.
Two camera positions at consecutive time instants $k-1$ and $k$ are related by a rigid body transformation:
\begin{equation}
T_t = T_{t,t-1} = 
\left[ \begin{array}{c c}
R_{t,t-1} & t_{t,t-1} \\
0 & 1
\end{array} \right]
\end{equation}
where $R_{t,t-1}$ is the rotation matrix and $t_{t,t-1}$ is the translation vector.
The set $T_{1:n} = \{T_1, \hdots, T_n\}$ contains all subsequent motions.
The set of camera poses $C_{0:n} = \{C_0, \hdots, C_n\}$ contains the transformation of the camera with respect to the initial coordinate frame at time $t = 0$.

The objective of Visual Odometry is to recover the full trajectory of the camera $C_{0:n}$ (i.e., egomotion).
This is achieved by computing the relative transformation $T_t$ from images $I_t$ and $I_{t-1}$.
When all relative transformation are concatenated, the full trajectory of the camera $C_{0:n}$ is recovered.
An iterative refinement procedure like \textit{windowed-bundle adjustment} can be performed to obtain a more accurate estimate of the local trajectory.

The first step of VO is the extraction of image features $f_t$ from a new image $I_t$.
These features are matched with features $f_{t-1}$ from the previous image (multiple images can be used to recover 3-D motion).
The third step consists of computing the relative motion $T_t$ between time $t-1$ and $t$.
For an accurate motion estimation, feature correspondences should not contain outliers, as described in Section \ref{sec:theory_feature_matching}.

The geometric relations between two images $I_t$ and $I_{t-1}$ are described by the \textit{essential matrix} $E$:
\begin{equation}
E_t \simeq \hat{t}_t R_t
\end{equation}
where $t_k = [t_x, t_y, t_z]^T$ and 
\begin{equation}
\hat{t}_t = 
\left[ \begin{array}{c c c}
0 & -t_z & t_y \\
t_z & 0 & -t_x \\
-t_y & t_x & 0
\end{array} \right]
\end{equation}
The rotation and translation of the camera can be extracted from $E$.
%At least five feature correspondences are required.
%An efficient implementation is proposed Nister \cite{nistér2004efficient}, which has become the standard for 2-D to 2-D motion estimation.
A simple solution when at least eight feature correspondences are available, is the Longuet-Higgins's eight-point algorithm \cite{longuet1987computer}.
Each feature correspondence gives a constraint of the following form:
\begin{equation}
\left[ \begin{array}{c c c c c c c c c}
\tilde{u}\tilde{u}' & \tilde{u}'\tilde{v} & \tilde{u}' & \tilde{u}\tilde{v}' & \tilde{v}\tilde{v}' & \tilde{v}' & \tilde{u}' & \tilde{v}' & 1
\end{array} \right]
E = 0
\end{equation}
where $E = [e_1  e_2  e_3  e_4  e_5  e_6  e_7  e_8  e_9]^T$.
Stacking these constraints gives the linear equation $A E = 0$, where $A$ is the camera instrinsic matrix.
This equation can be solved by using singular value decomposition (SVD), which has the form $A = USV^T$.
Now, the projected essential matrix $\overline{E}$ can be found as the last column of $V$:
\begin{equation}
\overline{E} = U diag\{1, 1, 0\} V_T
\end{equation}
The rotation and translation can be extraced from $\overline{E}$:
\begin{equation}
R = U (\pm W^T) V^T
\end{equation}
\begin{equation}
\hat{t} = U (\pm W^T) S U^T
\end{equation}
where
\begin{equation}
W^T =
\left[ \begin{array}{c c c}
0 & \pm 1 & 0 \\
\mp 1 & 0 & 0 \\
0 & 0 & 1
\end{array} \right]
\end{equation}
Now, the camera position at time $t$ can be recovered by concatenating the relative transformations:
\begin{equation}
C_t = \prod_{k=t}^1T_{k}
\end{equation}

Additional computations can be performed in order to retrieve the absolute scale of the translations.

